/**
* @file        NaviUtilTools
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2023/8/23 16:21
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#include "NaviAttitudeTools.hpp"
namespace UtilTools::Attitude{
    using RotationVector = Eigen::Vector3d;
    using Euler = Eigen::Vector3d; /*roll, pitch, heading*/

    /**
     * @brief  quaternion to rotation martrix by Eigen
     * @note
     * @param  q: quaternion
     * @retval rotation matrix
     */
    Eigen::Matrix3d Quaternion2RotationMatrix(const Eigen::Quaterniond &q) {
        return q.toRotationMatrix();
    }

    /**
     * @brief  rotation martrix to quaternion by Eigen
     * @note
     * @param  mat:
     * @retval
     */
    Eigen::Quaterniond RotationMartix2Quaternion(const Eigen::Matrix3d &mat) {
        Eigen::Quaterniond q(mat);
        return q;
    }

    /**
     * @brief
     * @note
     * @param  &euler:
     * @retval
     */
    Eigen::Quaterniond Euler2Quaternion(const Euler &euler) {
        return (Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX()));
    }

    /**
     * @brief  Euler angle to rotation matrix
     * @note
     * @param  euler: Euler angle
     * @retval
     */
    Eigen::Matrix3d Euler2RotationMatrix(const Euler &euler) {
        return Quaternion2RotationMatrix(Euler2Quaternion(euler));
    }

    /**
     * @brief  Rotation Matrix to Euler by Eigen
     * @note   note the order of the axis
     * @param  &mat:
     * @retval
     */
    Euler RotationMartix2Euler(const Eigen::Matrix3d &mat) {
        return eulerAngles(mat);
    }

    /**
     * @brief  quaternion to euler by rotation matirx
     * @note
     * @param  q: quaternion
     * @retval
     */
    Euler Quaternion2Euler(const Eigen::Quaterniond &q) {
        return RotationMartix2Euler(q.toRotationMatrix());
    }

    /**
     * @brief  according the rotation vector construct the Quaternion
     * @note
     * @param  rv: Equivalent rotation vector
     * @retval
     */
    Eigen::Quaterniond RotationVector2Quaternion(const RotationVector &rv) {
        Eigen::Quaterniond qfromrv;
        RotationVector rv_2 = rv * 0.5;
        double norm = rv_2.norm();
        qfromrv.w() = cos(norm);
        qfromrv.vec() = norm < 1e-8 ? rv_2 : (sin(norm) / norm) * rv_2;
        return qfromrv;
    }

    /***
     * 四元数转等效旋转矢量
     * @param quaternion
     * @return
     */
    Eigen::Vector3d Quaternion2Vector(const Eigen::Quaterniond &quaternion) {
        Eigen::AngleAxisd axisd(quaternion);
        return axisd.angle() * axisd.axis();
    }

    /**
     * @brief 将等效旋转矢量转为四元数
     * @param rotvec
     * @return
     */
    Eigen::Quaterniond RotVec2Quaternion(const Eigen::Vector3d &rotvec) {
        double angle = rotvec.norm();
        Eigen::Vector3d vec = rotvec.normalized();
        return Eigen::Quaterniond(Eigen::AngleAxisd(angle, vec));
    }

    /**
     * @brief  Rotation Vector to Rotation Matrix
     * @note
     * @param  &rv: Equivalent rotation vector
     * @retval
     */
    //    Eigen::Matrix3d RotationVector2RotationMatrix(const RotationVector &rv) {
    //        return Quaternion2RotationMatrix(RotationVector2Quaternion(rv));
    //    }

    /**
     * @brief  skew matrix from vector
     * @note
     * @param  vector:
     * @retval
     */
    Eigen::Matrix3d Skew(const Eigen::Vector3d &vector) {
        Eigen::Matrix3d dcm1;
        dcm1(0, 0) = 0;
        dcm1(0, 1) = -vector(2);
        dcm1(0, 2) = vector(1);

        dcm1(1, 0) = vector(2);
        dcm1(1, 1) = 0;
        dcm1(1, 2) = -vector(0);

        dcm1(2, 0) = -vector(1);
        dcm1(2, 1) = vector(0);
        dcm1(2, 2) = 0;

        return dcm1;
    }
    /**
     * 角度差值绝对值
     * @param angle1
     * @param angle2
     * @return
     */
    float AbsSubAngle(float angle1, float angle2) {
        float sub = fabs(angle1 - angle2);
        if (sub > 180) {
            sub = 360 - sub;
        }
        return sub;
    }

    /**
     * 角度折合0~2pi
     * @param radian rad
     * @return
     */
    float WrapRadian_2Pi(float radian) {
        double scale = radian / (M_PI * 2);

        if (scale >= 1) {
            radian -= (float) (M_PI * 2 * (int) scale);
        } else if (scale <= -1) {
            radian += (float) (M_PI * 2 * (int) scale);
        }

        if (radian < 0) {
            return (float) (M_PI * 2) + radian;
        }

        return radian;
    }
    /**
     * 角度投影向量
     * @param angle deg
     * @return
     */
    //        std::vector<float> VectorFromAngle(float angle) {
    //            std::vector<float> res;
    //            res.push_back((float) cos(angle * D2R));
    //            res.push_back((float) sin(angle * D2R));
    //
    //            return res;
    //        }

    /**
     * 角度折合到0~360
     * @param angle deg
     * @return
     */
    //        float WrapAngle(float angle) {
    //            double scale = angle / 360.0;
    //
    //            if (scale >= 1) {
    //                angle -= (float) (360 * (int) scale);
    //            } else if (scale <= -1) {
    //                angle += (float) (360 * (int) fabs(scale));
    //            }
    //
    //            if (angle < 0) {
    //                angle += 360;
    //            }
    //            return angle;
    //        }
    /**
     * 将num 映射到区间内
     * @param x
     * @param low
     * @param high
     * @return
     */
    //        float WrapNum(float x, float low, float high) {
    //            if (low <= x && x < high) {
    //                return x;
    //            }
    //            const auto range = high - low;
    //            const auto inv_range = 1.0 / range;
    //            const auto num_wraps = floor((x - low) * inv_range);
    //            return x - range * num_wraps;
    //        }
}